﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Complex;
using UnscentedKalmanFilter;

namespace FliterLearn
{

    public class AccModel : FilterHelp
    {
        public AccModel()
        {
            filter = new Filter();
            filter.X = CreateVector.DenseOfArray(new double[] { 0, 0, 0 });  // 初始值
            filter.F = CreateMatrix.DenseOfArray(new double[,] { { 1, 1, 0.5 }, { 0, 1, 0.5 }, { 0, 0, 1 } });
            filter.P = CreateMatrix.DenseOfArray(new double[,] { { 1000, 0, 0 }, { 0, 1000, 0 }, { 0, 0, 1000 } });
            filter.H = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });
            filter.Q = CreateMatrix.DenseOfArray(new double[,] { { 0.0001, 0, 0 }, { 0, 0.0001, 0 }, { 0, 0, 0.000001 } });
            filter.R = CreateMatrix.DenseOfArray(new double[,] { { 1000, 0, 0 }, { 0, 1000, 0 }, { 0, 0, 1000 } });
            filter.B = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });
        }
    }

    public class UniformModel : FilterHelp
    {
        public UniformModel()
        {
            filter = new Filter();
            filter.X = CreateVector.DenseOfArray(new double[] { 0, 0, 0 });  // 初始值
            filter.F = CreateMatrix.DenseOfArray(new double[,] { { 1, 1, 0 }, { 0, 1, 0 }, { 0, 0, 0 } });
            filter.P = CreateMatrix.DenseOfArray(new double[,] { { 1000, 0, 0 }, { 0, 1000, 0 }, { 0, 0, 0 } });
            filter.H = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 0 } });
            filter.Q = CreateMatrix.DenseOfArray(new double[,] { { 0.0001, 0, 0 }, { 0, 0.0001, 0 }, { 0, 0, 0 } });
            filter.R = CreateMatrix.DenseOfArray(new double[,] { { 800, 0, 0 }, { 0, 800, 0 }, { 0, 0, 0 } });
            filter.B = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });
        }
    }

    public class StaticModel : FilterHelp
    {
        public StaticModel()
        {
            filter = new Filter();
            #region 静止模型
            filter.X = CreateVector.DenseOfArray(new double[] { 5, 0, 0 });  // 初始值
            filter.F = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } });
            filter.P = CreateMatrix.DenseOfArray(new double[,] { { 1000, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } });
            filter.H = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } });
            filter.Q = CreateMatrix.DenseOfArray(new double[,] { { 0.000001, 0, 0 }, { 0, 0, 0 }, { 0, 0, 0 } });
            filter.R = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });
            filter.B = CreateMatrix.DenseOfArray(new double[,] { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } });
            #endregion

        }
    }

    public abstract class FilterHelp
    {
        protected Filter filter;
        public double[] last = { 0,0,0 };

        public virtual double Update(double z,double[] u = null)
        {
            double diff = z - last[0];
            double _2ndDiff = diff - last[1];
            if (u == null)
            {
                u = new double[] { 0, 0, 0 };
            }
            last = filter.Update(new double[] { z, diff, _2ndDiff }, u);
            return last[0];
        }
    }


    public class Filter
    {
        public Vector<double> X; // 初始估算值
        public Matrix<double> P; // 协方差
        public Matrix<double> F; // 状态外推系数
        public Matrix<double> H; // 观测系数
        public Matrix<double> R; // 测量误差
        public Matrix<double> Q; // 过程噪声 (模型的噪声)
        public Matrix<double> B; // 控制矩阵

        public double[] Update(double[] z,double[] u)
        {
            Vector<double> Z = CreateVector.DenseOfArray(z);
            Vector<double> U = CreateVector.DenseOfArray(u);
            /* 预测 */
            X = F * X + B * U;

            P = F * P * F.Transpose() + Q;
            /* 状态更新 */
            //var K = P * H.Transpose() * (H * P * H.Transpose() + R).Inverse(); // 卡尔曼增益
            var K =  P * H.Transpose() * (H * P * H.Transpose() + R).PseudoInverse(); // 卡尔曼增益
            X = X + K * (Z - H*X);
            P = P - K * H * P;
            return X.ToArray();
        }
    }
}
